/*
** my_iden.c
** 
** Made by (Alejandro)
** 
** Started on  Mon Jun 29 21:14:40 2009 Alejandro
*/

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>

#include "cosmoparam.h"

#define ERRNFILE 1
#define ERRUSAGE 2
#define ERRSTACK 3
#define ERRFILE  4

#define STACKSIZE (128 * 128 * 128) 

struct Particle {
  float x, y, z;
  int grp_tag;
  int grid_index;
  struct Particle *next;
};

typedef struct Particle Particle;

#define _INT_COORD(coord, box_size) (int) (coord*box_size)

/* pp0 and pp1 are pointers to particles */
#define EQ(pp0, pp1) ((pp0->x == pp1->x) && (pp0->y == pp1->y) && (pp0->z == pp1->z))

struct _Stack {
  int next_free;
  Particle **items;
};

typedef struct _Stack Stack;

#define empty(stack) (stack->next_free == 0)

#define full(stack) (stack->next_free == STACKSIZE)

Stack * new_stack(void)
{
  Stack * __stack;
  Particle * p;
  __stack = malloc(sizeof(Stack));
  __stack->next_free = 0;
  __stack->items = malloc(sizeof(p) * STACKSIZE);
  return __stack;
}

Particle * pop(Stack * stack)
{
  Particle * elem;
  if (empty(stack)) {
    printf("Error poping empty stack");
    exit(ERRSTACK);
  } else {
    elem = stack->items[(stack->next_free)-1];
    stack->next_free -= 1;
    return elem;
  }
}

void push(Stack * stack, Particle * elem)
{
  if (full(stack)) {
    printf("Error: stack is full! Try increasing STACKSIZE.\n");
    exit(ERRSTACK);
  }
  stack->items[stack->next_free] = elem;
  stack->next_free++;
  return;
}

void del(Stack * stack)
{
  free(stack->items);
  free(stack);
}

static int cmp_grid_idx(const void * elem0, const void * elem1)
{
  int gi0, gi1;

  gi0 = *((int *) elem0);
  gi1 = ((Particle *) elem1) -> grid_index;
  
  return (gi0 - gi1);
}


static int cmp_element(const void *elem0, const void *elem1)
{

  int gi0, gi1;

  gi0 = ((Particle *) elem0) -> grid_index;
  gi1 = ((Particle *) elem1) -> grid_index;
  
  return (gi0 - gi1);
}

typedef struct {
  /* One and only one of the bodies/particles with equal 
     GRID_INDEX (see below) can be inside a box. */
  Particle * particles;

  /* number of particles */
  int nobj;


  /* It's a square grid, so width, height and lenght are the same. */
  int size; 

  /* Each box of the grid represents "size/rlbox" volume of space. 
     That is each box is "size/rlbox" long by "size/rlbox" width 
     by "size/rlbox" high. */
  float rlbox;
} Grid;

#define BOX_SIZE(grid) (grid.size / grid.rlbox)

#define GRID_INDEX(particle, grid)					\
  ((_INT_COORD (particle.x, BOX_SIZE(grid)) * (grid.size) +		\
    _INT_COORD (particle.y, BOX_SIZE(grid))) * (grid.size) +		\
   _INT_COORD (particle.z, BOX_SIZE(grid)))


typedef struct {
  int      npart[6];
  double   mass[6];
  double   time;
  double   redshift;
  int      flag_sfr;
  int      flag_feedback;
  int      npartTotal[6];
  int      flag_cooling;
  int      num_files;
  double   BoxSize;
  double   Omega0;
  double   OmegaLambda;
  double   HubbleParam; 
  /* fills to 256 Bytes */
  char     fill[256 - 6*4 - 6*8 - 2*8 - 2*4 - 6*4 - 2*4 - 4*8];
} IOHeader;

void read_header(IOHeader * head, FILE * pf)
{
  int dummy;

  fread(&dummy, sizeof(dummy), 1, pf);
  fread(head, sizeof(IOHeader), 1, pf);
  fread(&dummy, sizeof(dummy), 1, pf);
}

void read_data(IOHeader * head, FILE * pf, Particle * particles,
	       int * id, int * ind)
{
  /* ind is used to read particles in the next file */
  int dummy, k, pc, n;
  float r[3],v[3];

  fread(&dummy, sizeof(dummy), 1, pf);
  fread(head, sizeof(IOHeader), 1, pf);
  fread(&dummy, sizeof(dummy), 1, pf);
  fread(&dummy, sizeof(dummy), 1, pf);

  double BoxSize = head->BoxSize;

  for (k = 0, pc = 0; k < 6; k++) {

    for (n = 0; n < head->npart[k]; n++) {

      fread(&r[0], sizeof(float), 3, pf);

      if (r[0] < 0.)
	r[0] += BoxSize;
      else if (r[0] >= BoxSize)
	r[0] -= BoxSize;

      if (r[1] < 0.)
	r[1] += BoxSize;
      else if (r[1] >= BoxSize)
	r[1] -= BoxSize;

      if (r[2] < 0.)    
	r[2] += BoxSize;
      else if (r[2] >= BoxSize) 
	r[2] -= BoxSize;

      particles[*ind+pc].x = r[0];
      particles[*ind+pc].y = r[1];
      particles[*ind+pc].z = r[2];

      pc++;
    }
  }

  fread(&dummy, sizeof(dummy), 1, pf);
  printf("pc pos %d\n",pc);
  fread(&dummy, sizeof(dummy), 1, pf);

  for (k = 0; k < 6; k++) {
    for (n = 0; n < head->npart[k]; n++) {
      fread(&v[0], sizeof(float), 3, pf);
      pc++;
    }
  }

  /* What is this for?

     I added this check cause it seems we are not using the "ids" */
  if (id != NULL) {

    fread(&dummy, sizeof(dummy), 1, pf);
    fread(&dummy, sizeof(dummy), 1, pf);
    for (k = 0, pc = 0; k < 6; k++) {
      for(n = 0; n < head->npart[k]; n++) {
	fread(&id[*ind+pc], sizeof(int), 1, pf);
	pc++;
      }
    }

    *ind+=pc;
  }
}

/** Grid and neighbor finding */
void grid(Grid * grid)
{

  int i, nobj, grid_index;
  Particle particle;
  Particle * last_p, * first_p;
  Particle * particles;

  particles = grid->particles;
  nobj = grid->nobj;

  for (i = 0; i < nobj; i++) {
    particle = particles[i];
    grid_index = GRID_INDEX(particle, (*grid)); 
    grid->particles[i].grid_index = grid_index;
  }  

  qsort(grid->particles, (size_t) nobj, sizeof(Particle), cmp_element);
 
  grid->particles[0].next = NULL;
  grid->particles[0].grp_tag = -1;

  last_p = &grid->particles[0];
  first_p = &grid->particles[0];

  for (i = 1; i < nobj; i++) {
    if (grid->particles[i].grid_index == last_p->grid_index) {
      last_p->next = &grid->particles[i];
      last_p = &grid->particles[i];
    } else {
      if (!(EQ(last_p, first_p)))
	last_p->next = first_p;
      else
	last_p->next = NULL;

      first_p = &grid->particles[i];
      last_p = &grid->particles[i];
    }
    grid->particles[i].grp_tag = -1;
  }

  if (!(EQ(last_p, first_p)))
    last_p->next = first_p;
  else
    last_p->next = NULL;
  return;
}


const int neighbours_offset[27][3] = {{-1, -1, -1},
				      {-1, -1,  0},
				      {-1, -1,  1},
				      {-1,  0, -1},
				      {-1,  0,  0},
				      {-1,  0,  1},
				      {-1,  1, -1},
				      {-1,  1,  0},
				      {-1,  1,  1},
				      { 0, -1, -1},
				      { 0, -1,  0},
				      { 0, -1,  1},
				      { 0,  0, -1},
				      { 0,  0,  0},
				      { 0,  0,  1},
				      { 0,  1, -1},
				      { 0,  1,  0},
				      { 0,  1,  1},
				      { 1, -1, -1},
				      { 1, -1,  0},
				      { 1, -1,  1},
				      { 1,  0, -1},
				      { 1,  0,  0},
				      { 1,  0,  1},
				      { 1,  1, -1},
				      { 1,  1,  0},
				      { 1,  1,  1}};


#define wrap(x, wrap)  (x < 0 ? wrap + x : (x >= wrap ? wrap - x : x))

int neighbours(Grid * grid, Particle * particle, float r0, int tag, Stack * stack) {
  /* grab a bunch of particles and check if they are neighbors */

  int i;
  int displaced_x, displaced_y, displaced_z;
  int x, y, z;
  int neighbour_index;
  int grid_size;
  int stop_cond;
  int new_neighbours;

  Particle * search_res, * cycle;

  float dis, dis_x, dis_y, dis_z, rlbox, rlbox2;

  rlbox = grid->rlbox;
  rlbox2 = rlbox/2.0;
  grid_size = grid->size;
  
  x = _INT_COORD(particle->x, BOX_SIZE((*grid)));
  y = _INT_COORD(particle->y, BOX_SIZE((*grid)));
  z = _INT_COORD(particle->z, BOX_SIZE((*grid)));
  
  new_neighbours = 0;
  for (i = 0; i < 27; i++) {
    displaced_x = x + neighbours_offset[i][0];
    displaced_y = y + neighbours_offset[i][1];
    displaced_z = z + neighbours_offset[i][2];

    displaced_x = wrap(displaced_x, grid_size);
    displaced_y = wrap(displaced_y, grid_size);
    displaced_z = wrap(displaced_z, grid_size);

    neighbour_index = (displaced_x * grid_size + displaced_y) 
      * grid_size + displaced_z;
    
    search_res = bsearch(&neighbour_index, grid->particles, grid->nobj,
			 sizeof(Particle), cmp_grid_idx);

    if (search_res != NULL) {
      cycle = search_res;
      do {
	if (cycle->grp_tag == -1 && !EQ(cycle, particle)) {

	  dis_x = cycle->x - particle->x;
	  dis_y = cycle->y - particle->y;
	  dis_z = cycle->z - particle->z;
	
	  if (dis_x > rlbox2) dis_x = dis_x - rlbox;
	  if (dis_y > rlbox2) dis_y = dis_y - rlbox;
	  if (dis_z > rlbox2) dis_z = dis_z - rlbox;
	  if (dis_x < -rlbox2) dis_x = dis_x + rlbox;
	  if (dis_y < -rlbox2) dis_y = dis_y + rlbox;
	  if (dis_z < -rlbox2) dis_z = dis_z + rlbox;
	
	  dis = sqrt(dis_x*dis_x + dis_y*dis_y + dis_z*dis_z);
	
	  if (dis < r0) {
	    push(stack, cycle);
	    cycle->grp_tag = tag;
	    new_neighbours += 1;
	  }
	}

	cycle = cycle->next;
	if (cycle == NULL)
	  stop_cond = 1;
	else  
	  stop_cond = EQ(cycle, search_res);
      } while (!stop_cond);
    }
  }

  return new_neighbours;
}

int fof(Grid *grid, float r0) {

  Stack * stack;
  int i, j, nobj, ngrp, nn; /* nn is number of neighbours found */
  Particle * elem;

  stack = new_stack();
  nobj = grid->nobj;

  ngrp = 1;
  for (i = 0; i < nobj; i++) {
    if (i % 100000 == 0) {
      printf(".");
    }
    
    if (grid->particles[i].grp_tag != -1) {
      continue;
    }
    else if (grid->particles[i].grp_tag == -1) {
      nn = 0;
      push(stack, &(grid->particles[i]));
      grid->particles[i].grp_tag = ngrp;
      while (!empty(stack)) {
	elem = pop(stack);
	nn += neighbours(grid, elem, r0, ngrp, stack);
      }
      
      if (nn != 0) {
	ngrp++;
      } else {
	grid->particles[i].grp_tag = 0;
      }
    }
  }

  del(stack);
  return ngrp-1;
}

/* This method is similar to the original one */
int fof2(Grid * grid, float r0) {
  int   i, j, k;
  int   *lirst, *ll;
  int   *listvec, *listvecnew;
  int   ngrid;
  int   nvec, nvecvec;
  int   ngrupos;
  int nobj;
  Stack * stack, * _stack;

  stack = new_stack();
  _stack = new_stack();

  nobj = grid->nobj;

  printf("Empieza Iden\n");
  fflush(stdout);

  ngrupos = 1;

  for( i = 0 ; i < nobj; i++)
  {
    if (i % 100000 == 0) {
      printf(".");
      fflush(stdout);
    }
    
    if (grid->particles[i].grp_tag != -1)
      continue; /* salta a la siguiente */
    
    nvec = neighbours(grid, &grid->particles[i], r0, ngrupos, stack);
    
    assert(nvec == stack->next_free);

    if (nvec != 0) {
      grid->particles[i].grp_tag = ngrupos;
    } else {
      grid->particles[i].grp_tag = 0;
    }

    do
    {
      nvecvec = 0;
      int tmp, last_top;
      for (j = 0; j < stack->next_free; j++) {
	tmp = neighbours(grid, stack->items[j], r0, ngrupos, _stack);
	nvecvec += tmp;
      }
      assert(nvecvec == _stack->next_free);

      if (nvecvec != 0) {
	stack->next_free = 0;
	for (k = 0; k < _stack->next_free; k++) {
	  push(stack, _stack->items[k]);
	}
	assert(stack->next_free == _stack->next_free);
      }

      _stack->next_free = 0;
    } while( nvecvec != 0 ); /*fin del do de los vecinos de los vecinos*/
    stack->next_free = 0;
    
    if (nvec != 0)
      ngrupos++;


  }

  del(stack);
  del(_stack);
  printf("termino identificacion\n");

  return ngrupos;
}

double deltavir( double z )
{
    double q ;
    double as, x ;
    as=1./(1.+z);
    if ((cp.omegam == 1.) && (cp.omegal == 0.))
    {
        q = 177.7 ;      
        /* Einstein - de Sitter Model Peebles, 1980. Eq 19.50 */
    }
    if ((cp.omegam < 1.) && (cp.omegal == 0.))
    {
        x = as * 2. *(1./cp.omegam-1.);
        q = pow(2.*M_PI,2.)*pow(x,3.)/pow(sqrt(x*(x+2.))-log((1.+x)+sqrt(x*(x+2.))),2.);
        /* Maoz, 1990 Eq 12. Lacey & Cole, 1993 Eq A15. Oukbir & Blanchard, 1997 */
    }
    if ((cp.omegam < 1.) && (cp.omegak == 0.))
    {
        x = as*pow(1./cp.omegam-1.,1./3.);
        q = 18.*M_PI*M_PI*(1.+0.4093*pow(x,2.71572)); 
        /* Nakamura & Suto, 1997 Eq c-19. Eke et al, 1996 */
    }
    return q;
}



int main(int argc, char * argv[])
{
  
  int i, l, gn, ind = 0;
  int nobj;
  int * id = NULL, *npgrupo;
  float rlbox, redshift;
  int iper;
  int *grupo;
  double r0;
  char filename[200];
  FILE *pfout, *pf;
  int k;
  int numerodegrupos, ifile;
  int *cabecera, *lista;
  int tag;

  IOHeader head;

  if (argc != 3) {
    printf("usage: iden <file> <outfile>\n");
    exit(ERRUSAGE);
  }

  /* open file to read */

  sprintf(filename, "%s", argv[1]);
  printf("Reading file: %s\n", filename);

  if ( !(pf = fopen(filename, "r"))) {
    printf("Can't open file `%s`\n", filename);
    exit(ERRNFILE);
  }
  
  /* read the head of the file and extract useful data */
  read_header(&head, pf);

  nobj = 0;
  for (k = 0; k < 6; k++)
    nobj += head.npartTotal[k];

  rlbox = head.BoxSize;
  redshift = head.redshift;

  printf("Numero de particulas: %d Box en kpc: %f redshift: %f\n",
  	 nobj, head.BoxSize, head.redshift);
  printf("Omegas %f %f\n", head.Omega0, head.OmegaLambda);

  cp.omegam = head.Omega0;
  cp.omegal = head.OmegaLambda;
  cp.omegak = 1. - cp.omegam - cp.omegal;

  /* end of reading file header */
  
  Particle * particles;
  particles = malloc(nobj*sizeof(Particle));
  grupo = malloc(nobj*sizeof(int));

  rewind(pf);
  read_data(&head, pf, particles, NULL, &ind);
  fclose(pf);

  /* end of input, data in memory */
  
  iper = 1;
  r0 = rlbox/pow((float) nobj, 0.33333) *	\
    pow(1.0/(0.4*deltavir(redshift)), 0.33333);

  Grid _grid;
  int ngrp;
  
  printf("r0 = %f\n", r0);

  _grid.particles = particles;
  _grid.nobj = nobj;
  _grid.size = 512;
  _grid.rlbox = rlbox;
  
  printf("rlbox: %f\n", rlbox);

  grid(&_grid);
  ngrp = fof(&_grid, r0);
  
  printf("Number of groups: %d\n", ngrp);

  cabecera   = malloc(ngrp*sizeof(int));
  lista      = malloc(nobj*sizeof(int));
  npgrupo    = malloc(ngrp*sizeof(int));

  for (i = 0; i < ngrp; i++) {
    cabecera[i] = 0;
    npgrupo[i] = 0;
  }

  for (i = 0; i < nobj; i++)
    lista[i] = 0;

  for (i = 0; i < nobj; i++) {
    tag = _grid.particles[i].grp_tag;
    cabecera[tag] = i;
    npgrupo[tag]++;
  }

  for (i = 0; i < nobj; i++) {
    tag = _grid.particles[i].grp_tag;
    lista[cabecera[tag]] = i;
    cabecera[tag] = i;
  }

  sprintf(filename, "./data/%s.iden.dat", argv[2]);
  if ( !(pfout = fopen(filename, "w"))) {
    printf("Can't open file `%s`\n", filename);
    exit(ERRFILE);
  }

  gn = 0;
  for (i = 1; i < ngrp; i++)
  {
    if (npgrupo[i] < 10)
      continue;
    fwrite(&npgrupo[i], sizeof(int), 1, pfout);
    l = cabecera[i];
    do {
      l = lista[l];
      fwrite(&l, sizeof(int), 1, pfout);
    } while (l != cabecera[i]);
    fwrite(&npgrupo[i],sizeof(int),1,pfout);
    gn++;
  }
  printf("numero total de grupos com mas de 10: %d\n",gn);
  fclose(pfout);

  return 0;
}

